//
// Created by de on 3/29/21.
//

#ifndef FORCEEIGENTEST_ROSHANDLER_H
#define FORCEEIGENTEST_ROSHANDLER_H

#include <ros/node_handle.h>
#include <hirop_msgs/forceCtlDevStatus.h>
#include "hsr_rosi_device/SetEnableSrv.h"
#include "memory"
//#include "
#include "Global/GlobalVar.h"

/**
 *  ROS  controoller
 * */






class RosHandler {
public:
    RosHandler();
    explicit RosHandler(ros::NodeHandle* node);

    bool sendDeviceInit();
    bool sendDeviceCheck();
    bool sendDeviceStart();
    bool sendDeviceStop();
    bool sendDeviceReset();
    bool sendDeviceForceReset();

    bool sendDeviceEnable();
    VisionMeasure sendVisionMeaDetect();

    ForceStateData getDeviceControlStateStatus();

    ForceDeviceCommCode getDeviceCommuStatus();
private:
    ros::NodeHandle* node;

    ros::ServiceClient server_launchPiH;
    ros::ServiceClient server_startPiH;
    ros::ServiceClient server_stopPiH;
    ros::ServiceClient server_resetPiH;
    ros::ServiceClient server_forcedresetPiH;
    ros::ServiceClient server_getStatusPiH;
    ros::ServiceClient server_testPiH;
    ros::ServiceClient setEnableClient;
    ros::ServiceClient VisionMeasureCli;

    ros::Subscriber forceStatus_sub;

private:
    void forceStatus_Callback( const hirop_msgs::forceCtlDevStatus::ConstPtr& msg);
    std::vector<int> v_Error;

};


#endif //FORCEEIGENTEST_ROSHANDLER_H
